#ifndef _AUTOPILOT_H
#define _AUTOPILOT_H
#ifndef M_PI
#define M_PI 3.14159265358979
#endif
#define AP_DEBUG 0
#define AP_PWPT_DEBUG 1
#define SENS_SCAN 1
#define SPIT_POSE_M 1

#include "GluePose.h"
#define SIMU_MRPT 1 // 0 hors simulation
#if SIMU_MRPT
#include "GlueScan.h"
#else
#include "telemetre.h"
#include "math.h"
#include <vector>
using namespace std;
#endif
#include "linalg.h"

#include <iostream>

enum enum_sens {
	SENS_INDIRECT = -1, SENS_DIRECT = 1
};
/*
 * Classe d'analyse de scan
 * MODE D'EMPLOI
 *
 * Executer analyserScan:
 * - vérifie si le scan a le bon nombre de points
 * - vérifie si le scan est différent du précédent stocké [fonction faut-il analyser]
 * 	Si le scan doit être analysé
 * - il est copié dans la classe, dtms et scan sont mis à jour
 * - il est filtré sur place
 * - il est interprété (détection des poteaux)
 * - il est aplati (redressage des poteaux)
 * Le résultat de l'analyse est stocké dans le vecteur "pylones"
 */
class AP_AnalyseScan {
private:
	TypeScan scan; // copie du scan
	long int dtms; // temps en ms entre ce scan et le scan précédent

	// Fonctions usuelles
	void scancpy(const TypeScan _scan); // copie _scan dans scan
	void filtrerScan(TypeScan& scan_, float seuil); // élimine certaines données incohérentes du scan
	void interpreter(
			const TypeScan& _scan,
			float cos_pitch,
			float cos_roll,
			float diametre,
			float rcontig,
			vector<GPoint2D>& pylones_); // renvoie dans pylones la liste des poteaux vus
	inline void aplatir(
			vector<GPoint2D>& _pylones,
			float cos_pitch,
			float sin_pitch,
			float cos_roll,
			float sin_roll); // remet à plat les coordonnées en fonction de phi et theta
	inline bool fautilanalyser(const TypeScan& _scan, int nbpointsnominal); // détermine si le nouveau scan doit être évalué

	// Fonctions utilitaires
	inline float distTps(TypePointScan a, TypePointScan b);
public:
	AP_AnalyseScan();
	~AP_AnalyseScan();
	vector<GPoint2D> pylones;
	inline long int dt(){
		if (AP_DEBUG) cout << "inline long int AP_AnalyseScan::dt" << endl;
		return dtms;; // renvoie dtms
	}
	inline float dtfloat(){
		return (float) (dtms/1000.0);
	}
	/*
	 * Fonction analyserScan:
	 * renvoie vrai si le scan a été analysé, faux sinon
	 * si le scan a été analysé, renvoie la liste de pylones dans "pylones"
	 * on a le choix de stocker le résultat dans la structure ou ailleurs
	 */
	inline bool analyserScan(
			const TypeScan& _scan,
			float seuil,
			int nbpointsnominal,
			float phi,
			float theta,
			float diametre,
			float rcontig); // executer sur place, effets de bord
	bool analyserScan(
			const TypeScan& _scan,
			float seuil,
			int nbpointsnominal,
			float phi,
			float theta,
			float diametre,
			float rcontig,
			vector<GPoint2D>& pylones_); // sortie dans pylones_
};

class AP_ReconRepere {
public:
	AP_ReconRepere();
	~AP_ReconRepere();

	vector<GPoint2D> reperes;
	vector<bool> ident;

	vector<GPoint2D> pylid;
	vector<GPoint2D> repid;

	void ajouterRepere(GPoint2D repere);
	/*
	 * match:
	 * compare le vecteur pylones avec le vecteur reperes en les matchant à partir des données x, y et yaw
	 * sort deux listes ordonnées de pylones identifiés et repères identifiés
	 * pylnd renvoie la liste des pylones détectés non répertoriés.
	 */
	void match(
			float x,
			float y,
			float yaw,
			float rdetect,
			const vector<GPoint2D>& pylones,
			const vector<GPoint2D>& _reperes,
			vector<bool>& ident_,
			vector<GPoint2D>& pylid_,
			vector<GPoint2D>& repid_);// version utilisant des données externes
	void match(float x, float y, float yaw, float rdetect, const vector<
			GPoint2D>& pylones);// version utilisant les données internes
};

/*
 * Classe AP_Estimer
 * prend en entrée pylid et repid
 * comme paramètres yaw corrigé et yaw_offset
 * comme sorties position et vitesses, corrige yaw_offset si plus de 2 repères.
 */
class AP_Estimer {
private:
	short int pyltoarray(
			vector<GPoint2D> _pylones,
			alglib::real_2d_array& out,
			alglib::ae_int_t& m,
			alglib::ae_int_t& n);// transforme un vecteur "pylone" en matrice
	short int reptoarray(vector<GPoint2D> _repid, alglib::real_2d_array& out);// renvoie les repères dans un vecteur
	bool pinv(
			alglib::ae_int_t m,
			alglib::ae_int_t n,
			alglib::real_2d_array& in,
			alglib::real_2d_array& out,
			double tolerance); //pseudo inverse
	inline GPoint3D arraytopose(alglib::real_2d_array& in); // transforme le vecteur position alglib en x,y,yaw
public:
	GPoint2D pose; // position précédente (x,y)
	GPoint2D vit; // vitesse
	bool estim_val;
	/*
	 * Estimer: calcule x, y, et yaw_offset s'il y a lieu
	 * renvoie si l'estimation calculée est valable ou non
	 */
	bool estimer_pose(
			const vector<GPoint2D>& pylid,
			const vector<GPoint2D>& repid,
			float& x,
			float xp,
			float& y,
			float yp,
			float& vx,
			float& vy,
			float yaw,
			float& yaw_offset,
			long int dtms);
	bool estimer_pose(
			const vector<GPoint2D>& pylid,
			const vector<GPoint2D>& repid,
			float yaw,
			float& yaw_offset,
			long int dtms);
};

class AP_Estimateur {
public:
	AP_Estimateur();
	GPose3D pose_e;
	GPoint3D v_e;
	float kx, kv;
	void prediction(
			GPose3D pose_p,
			GPoint3D v_p,
			long int dtms,
			GPose3D& pose_s,
			GPoint3D& v_s);
	void
	estimateur(
			GPose3D pose_m,
			GPoint3D v_m,
			GPose3D pose_p,
			GPoint3D v_p,
			long int dtms,
			float kx,
			float kv,
			GPose3D& pose_e_,
			GPoint3D& v_e_);
	void estimateur(
			GPose3D pose_m,
			GPoint3D v_m,
			long int dtms,
			float kx,
			float kv);
};
/*
 * Contenant pour toute la localisation
 */
class AP_LocalisationWrapper {
public:
	AP_LocalisationWrapper();
	GPose3D pose_m;
	GPoint3D v_m;
	// variables à conserver
	AP_AnalyseScan analysescan;
	AP_ReconRepere reconrepere;
	AP_Estimer estimer;
	AP_Estimateur estimateur;
	float yaw_offset;

	// estimation;
	/*
	 * loc: tente de faire une estimation et renvoie vrai en cas de succès
	 * ajouter les coordonnées des repères avant
	 */
	void lireHYawPitchRoll(float h, float yaw, float pitch, float roll); // recopie les valeurs dans cette classe
	bool loc(
			const TypeScan _scan,
			int nbpointsnominal,
			float seuil,
			float pitch,
			float roll,
			float yaw,
			float pose_z,
			float diametre,
			float rcontig,
			float rdetect);

	inline float getX() {
		return pose_m.x();
	}
	inline float getY() {
		return pose_m.y();
	}
	inline float getVx() {
		return v_m.x;
	}
	inline float getVy() {
		return v_m.y;
	}
	inline float getXe() {
		return estimateur.pose_e.x();
	}
	inline float getYe() {
		return estimateur.pose_e.y();
	}
	inline float getVxe() {
		return estimateur.v_e.x;
	}
	inline float getVye() {
		return estimateur.v_e.y;
	}

};

class AP_Integrateur {
public:
	float val;
	inline float sat(float val, float max, float min) {
		if (val > max) return max;
		if (val < min) return min;
		return val;
	}
	inline float majInteg(float valent, float valc, float sat_e, float sat_s) {
		float incr;
		incr = sat(valent - valc, sat_e, -sat_e);
		val += incr;
		val = sat(val, sat_s, -sat_s);
	}
	inline void init() {
		val = 0;
	}
};

class AP_PiloteHuit {
private:

	// utilitaire
	inline float sat(float val, float norm) {
		if (val > norm) return norm;
		if (val < -norm) return -norm;
		return val;
	}
	inline GPoint3D sat(GPoint3D dir, float norm){
		return GPoint3D((float) sat(dir.x,norm),(float) sat(dir.y,norm),0);
	}
	// PiloteHuit
	int etape;
	inline void ligneDroite(
			GPose3D pose_e,
			GPoint3D v_e,
			GPoint2D depart,
			GPoint2D destination,
			float v_consigne,
			float K_r,
			float K_a,
			float K_m,
			float& a_cx,
			float& a_cy,
			float& a_consigne,
			float& gamma);
	inline void virage(
			GPose3D pose_e,
			GPoint3D v_e,
			GPoint2D centre,
			float rvirage,
			float v_consigne,
			enum_sens sens,
			float K_r,
			float K_a,
			float K_m,
			float& a_cx,
			float& a_cy,
			float& a_consigne,
			float& delta);
	void calculAccelC(
			GPose3D pose_e,
			GPoint3D v_e,
			int etape,
			float rvirage,
			float midistance,
			float vlent,
			float vrapide,
			float Kr,
			float Km,
			float& a_cx_,
			float& a_cy_,
			float& a_consigne_,
			float& psi_c_);// calcule l'accélération de rappel en fonction du tour
	int etapeSuivte(int etape, float rvirage, float midistance, GPose3D pose_e); // calcule l'étape suivante;
	void calculYawPitchRollC(
			float _a_cx,
			float _a_cy,
			float _a_consigne,
			float _xi_sat,
			float& _theta_c,
			float& _phi_c,
			float& _psi_c,
			float yaw,
			float yaw_offset);
	//PiloteWpt
	vector<GPoint3D> waypts;
	unsigned int cur_wpt;
	// Consignes
	float phi_c, theta_c, psi_c, psi_rate_c;
	// Intégrateurs piloteZero
	float vxZincr, vyZincr;
	// Intégrateurs piloteHuit
	AP_Integrateur distance_dt;
public:
	AP_PiloteHuit();
	~AP_PiloteHuit();
	void piloteHuit(
			GPose3D pose_e,
			GPoint3D v_e,
			float yaw,
			float yaw_offset,
			float rvirage,
			float midistance,
			float Kr,
			float Km,
			float vrapide,
			float vlent,
			float xisat);
	void piloteZero(
			GPose3D pose_m,
			GPoint3D v_m,
			float yaw_offset,
			float kp,
			float ki,
			float kd,
			float e_int_sat,
			float s_int_sat,
			float dt,
			float ang_max_deg);
	void piloteWpt(
			GPose3D pose_m,
			GPoint3D v_m,
			float yaw_offset,
			float kp,
			float ki,
			float kd,
			float e_int_sat,
			float s_int_sat,
			float dt,
			float ang_max_deg);
	void piloteWpt2(
			GPose3D pose_m,
			GPoint3D v_m,
			float yaw_offset,
			float kp,
			float ki,
			float kd,
			float e_int_sat,
			float s_int_sat,
			float dt,
			float ang_max_deg);
	void generateHuitWpts();
	void generateBonjourPoteau();
	inline float getPsiC() {
		return psi_c;
	}
	inline float getThetaC() {
		return theta_c;
	}
	inline float getPhiC() {
		return phi_c;
	}
	inline float getPsiRateC(float curpsi, float dt) {
		float psicorr = psi_c;
		float rep;
		// choix du représentant de psi_c le plus proche de curpsi
		if (abs(psicorr + 2 * M_PI - curpsi) < abs(phi_c - curpsi)) rep += 2
				* M_PI;
		else if (abs(psicorr - 2 * M_PI - curpsi) < abs(phi_c - curpsi)) rep
				-= 2 * M_PI;
		rep = psicorr - curpsi;
		rep *= dt;
		return rep;
	}
};

#endif
